%a pointset in frame i
Xi=[0 0 0;0 0 1; 0 1 0;1 0 0]'

%the relative pose between i and c
q=randn(4,1)
Tic=randn(3,1)
[Ric]=quater2rot(q)
%transform points to frame c
Xc=Ric*Xi+repmat(Tic,1,4)

%check
[R2ic,T2ic]=p3RT(Xi,Xc);

%the relative pose between i in time instant 1 and i in time instant 2
q=randn(4,1)
T1=randn(3,1)
%gather transformations for optimisation
T1t=T1';
R1t=q';
[R1]=quater2rot(q)
%project points from i0 to i1
Xi1=R1*Xi+repmat(T1,1,4)
%project points from i1 to c1
Xc1=Ric*Xi1+repmat(Tic,1,4)


%lets calculate what the trasformation should be from the pointsets
[R1c,T1c]=p3RT(Xc,Xc1);
%gather transformations for optimisation
R1ct=rot2quater(R1c);
T1ct=T1c';
%the equations for predicted rotation and translation in c frame
Rich=Ric*R1*Ric'
Tich=-Ric*R1*Ric'*Tic+Ric*T1+Tic

%the error

Rich-R1c 
Tich-T1c

%lets generate second movement
%the relative pose between i in time instant 1 and i in time instant 2
q=randn(4,1)
T1=randn(3,1)
%gather transformations for optimisation
T1t=[T1';T1t];
R1t=[q';R1t];
[R1]=quater2rot(q)
%project points from i0 to i1
Xi1=R1*Xi+repmat(T1,1,4)
%project points from i1 to c1
Xc1=Ric*Xi1+repmat(Tic,1,4)


%lets calculate what the trasformation should be from the pointsets
[R1c,T1c]=p3RT(Xc,Xc1);
%gather transformations for optimisation
R1ct=[rot2quater(R1c);R1ct];
T1ct=[T1c';T1ct];
%the equations for predicted rotation and translation in c frame
Rich=Ric*R1*Ric'
Tich=-Ric*R1*Ric'*Tic+Ric*T1+Tic

%the error

Rich-R1c 
Tich-T1c

%initialise parameter vector
X0=randn(1,7)
%cost function
f = @(x)myfun(x,R1t,T1t,R1ct,T1ct)
%optimise
X=fminunc(f,X0)
%evaluate solution
F=myfun(X,R1t,T1t,R1ct,T1ct)

%initialise parameter vector
X0=randn(1,9)
%cost function
f = @(x)myfunsc(x,R1t,T1t,R1ct,T1ct)
%optimise
X=fminunc(f,X0)
%evaluate solution
F=myfun(X,R1t,T1t,R1ct,T1ct)

X0=[randn(1,4) zeros(1,3) ones(1,40)/1.3];
%cost function
f = @(x)myfunsc(x,mpose(1:40,1:4),mpose(1:40,5:7),rpose(1:40,1:4),rpose(1:40,5:7))
X=fminunc(f,X0)

X0=[X(1:7) ones(1,180)/1.3];
%cost function
f = @(x)myfunsc(x,mpose(1:180,1:4),mpose(1:180,5:7),rpose(1:180,1:4),rpose(1:180,5:7))
tic;X=fminunc(f,X0);toc
X0=randn(1,7);
%cost function
f = @(x)myfun(x,lpose(1:180,1:4),lpose(1:180,5:7),opose(1:180,1:4),opose(1:180,5:7))
X=fminunc(f,X0)
f(X)
Xo=X

f = @(x)myfun(x,lpose(200:380,1:4),lpose(200:380,5:7),opose(200:380,1:4),opose(200:380,5:7))
f(X)

X0=randn(1,7);
%cost function
f = @(x)myfun(x,lpose(1:380,1:4),lpose(1:380,5:7),opose(1:380,1:4),opose(1:380,5:7))
X=fminunc(f,X0)
f(X)
